#include "RotationDetector.h"
#include "Logger.h"
#include "Observer.h"

RotationDetector::RotationDetector(Logger &_logger) : logger(_logger), yaw_kf(2, 1, 0)
{
    // clang-format off
    // 初始化 yaw_kf 滤波器参数
    yaw_kf.transitionMatrix = (cv::Mat_<float>(2, 2) << 1, 1, 0, 1);
    yaw_kf.measurementMatrix = cv::Mat::eye(1, 2, CV_32F);
    yaw_kf.processNoiseCov = cv::Mat::eye(2, 2, CV_32F) * 1e-3F;
    yaw_kf.measurementNoiseCov = cv::Mat::eye(1, 1, CV_32F) * 1e-2F;
    yaw_kf.errorCovPost = cv::Mat::eye(2, 2, CV_32F);
    yaw_kf.statePost = (cv::Mat_<float>(2, 1) << 0, 0);

    // clang-format on

    cv::FileStorage fs("./../Configure/ObserverSetting.xml", cv::FileStorage::READ);
    if (fs.isOpened())
    {
        fs["yaw_jump_threshold"] >> yaw_jump_threshold;
        fs["jump_count_threshold"] >> jump_count_threshold;
        fs.release();
    }
    else
    {
        logger.log("⚠️ Failed to open ObserverSetting.xml, using default thresholds.");
        abort();
    }
#ifdef DEBUG
    logger.logFormatted("yaw_jump_threshold : %f", yaw_jump_threshold);
    logger.logFormatted("jump_count_threshold : %d", jump_count_threshold);
#endif
}

bool RotationDetector::detect(const std::vector<float> &yaw_data, MotionStatus &out_status)
{
    int jump_count = 0;
    float last_yaw = 0.0F;
    bool first = true;
    MotionStatus tmp_status = MotionStatus::Unknown;

    for (float yaw_measured : yaw_data)
    {
        cv::Mat prediction = yaw_kf.predict();
        cv::Mat measured_yaw = (cv::Mat_<float>(1, 1) << yaw_measured);
        cv::Mat estimate = yaw_kf.correct(measured_yaw);

        cv::Mat residual = measured_yaw - prediction.row(0);
        cv::Mat residual_product = residual.t() * yaw_kf.measurementNoiseCov.inv() * residual;
        float mahalanobis_dist = std::sqrt(residual_product.at<float>(0, 0));
        if (mahalanobis_dist > yaw_jump_threshold)
        {
            jump_count++;

#ifdef DEBUG
            logger.logFormatted("Jump #%d detected: yaw %.2f -> %.2f, mahalanobis_dist: %.3f", jump_count, last_yaw,
                                yaw_measured, mahalanobis_dist);
#endif // DEBUG
            if (!first)
            {
                tmp_status = (last_yaw < yaw_measured) ? MotionStatus::RotatingClockwise
                                                       : MotionStatus::RotatingCounterClockwise;

#ifdef DEBUG
                logger.logFormatted("Motion status set to: %s", toString(tmp_status).c_str());
#endif // DEBUG
            }
            else
            {
                tmp_status = MotionStatus::Unknown;
            }

            yaw_kf.statePost = (cv::Mat_<float>(2, 1) << yaw_measured, 0);
            yaw_kf.errorCovPost = cv::Mat::eye(2, 2, CV_32F);
        }

        last_yaw = yaw_measured;
        first = false;
    }

    if (jump_count >= jump_count_threshold)
    {
        out_status = tmp_status;
        return true;
    }
    else
    {
        out_status = MotionStatus::Unknown;
        return true;
    }
}